package com.exc4.jbv;

import lejos.addon.gps.SimpleGPS;
import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.LCD;
import lejos.nxt.SensorPort;

public class PIDline {
	
	private static int arrSize = 40;

    private static int[] colorArray = new int[arrSize];
    private static ThreeColorSensor sensor;
	
	public static boolean isColor()
	{		
		for(int i= 0; i<arrSize; i++)
		{
			if(!sensor.isThirdColor(colorArray[i]))
			{
				return false;
			}
		}
		
		return true;
	}
	
	private static void exit()
	{
		
		Car.stop();
	    LCD.clear();
	    LCD.drawString("Program stopped", 0, 0);
	    LCD.refresh();
	    System.exit(0);	
	}
	
	public static void main (String[] aArg) throws Exception
	{
		final int power = 80;
		Button.ESCAPE.addButtonListener(new ButtonListener() {
			
			@Override
			public void buttonReleased(Button b) {	
				exit();
			}
			
			@Override
			public void buttonPressed(Button b) {
				// TODO Auto-generated method stub
				
			}
		});
	  
     sensor = new ThreeColorSensor(SensorPort.S2, "Green");

	 
     sensor.calibrate();
     
     for(int i = 0; i < arrSize; i++)
    	 colorArray[i] = 0;
     
     double kp = 0.08;
     double ki = 0.00075;
     double kd = 0.2;
     double offset = sensor.getOffset();
     double tp = 75;
     double integral = 0;
     double lastError = 0;
     double derivative = 0;
     double error = 0;
     double turn = 0;
     
     int lightValue = 0;
     int td = 10;
     
     int powerLeft = 0, powerRight = 0;
     int count = 0;
     
     LCD.clear();
     LCD.drawString("Light", 0, 0);
     LCD.drawString("Error", 0, 1);
     LCD.drawString("Left", 0, 2);
     LCD.drawString("Right", 0, 3);
     LCD.drawString("Offset", 0, 4);
     LCD.drawInt((int)offset, 7, 4);
     LCD.drawString("Lower", 0, 5);
     LCD.drawString("Upper", 0, 6);
     LCD.drawInt(sensor.getLower(), 6, 5);
     LCD.drawInt(sensor.getUpper(), 6, 6);
     LCD.drawString("Green", 0, 7);
     
     
	 int green = 0;
     while (true)
     {
    	 long time = System.currentTimeMillis();
    	 lightValue = sensor.light();
    	 
    	 colorArray[count++%arrSize] = lightValue;
    	 
    	 
    	 error = (double)lightValue - offset;
    	 
    	 integral += error;
    	 
    	 derivative = error - lastError;
    	 
    	 turn = kp * error + ki * integral + kd * derivative;
    	 
    	 //turn /= 100;
    	 
    	 powerLeft = (int)(tp + turn);
    	 powerRight = (int)(tp - turn);
    	 
    	 lastError = error;
    	 Car.forward(powerLeft, powerRight);
    	 
    	 LCD.drawInt(lightValue, 4, 7, 0);
    	 LCD.drawInt((int)error, 4, 7, 1);
    	 LCD.drawInt(powerLeft, 4, 7, 2);
    	 LCD.drawInt(powerRight, 4, 7, 3);
    	 
    	 if(green <= arrSize)
    	 {
    		if(sensor.isThirdColor())
    			 green++;
    		 else{
    			green = 0;
    		 }
    		 
    		LCD.drawInt(green, 6, 7);
    		if(green == arrSize)
    		 {
    			 while(!Button.ENTER.isPressed())
    	    			Car.stop();
    			 
    			 integral = 0;
    			 lastError = 0;
    			 derivative = 0;
			     error = 0;
			     turn = 0;
			     green = 0;			     
    		 }
    	 }
    	 	 
    	 /*if(isColor())
    	 {	 
    		 while(!Button.ENTER.isPressed())
    			Car.stop();
    		//exit();
    		 for(int i = 0; i < arrSize; i++)
    	    	 colorArray[i] = 0;
    	 }*/
	     
	     Thread.sleep(td);
	     
	     if(count == 500)
	     {
	    	 count = 0;
	     }
	     
     }
   }
}
